﻿using System;
using System.Diagnostics.Contracts;
using MathService.Values;

namespace MathService.DSP
{
    /// <summary>Цифровой фильтр с бесконечной импульсной характеристикой</summary>
    public class CBIH : DigitalFilter, IDigitalFilter, IValueRead<double>, IInitializable<double[], double[]>
    {
        /// <summary>Коэффициенты прямого фильтра</summary>
        private double[] f_a;
        /// <summary>Коэффициенты реверсивного фильтра</summary>
        private double[] f_b;

        /// <summary>Текущее щначение на выходе фильтра</summary>
        public double Value
        {
            get
            {
                var lv_Summ = .0;
                var i_Length = Order;
                for(var i = 0; i < i_Length; i++)
                    lv_Summ += f_DelayLine[i] * f_a[i];
                return lv_Summ;
            }
        }

        /// <summary>Фильтр с бесконечной импульсной характеристикой</summary>
        /// <param name="a">Массив коэффициентов прямого фильтра</param>
        /// <param name="b">Массив коэффициентов реверсивного фильтра</param>
        public CBIH(double[] a, double[] b)
            : base(Math.Max(a == null ? 0 : a.Length, b == null ? 0 : b.Length))
        {
            Initialize(a, b);
        }

        /// <summary>Иницифлизация начального состояния фильтра с новыми коэффициентами</summary>
        /// <param name="a">Массив коэффициентов прямого фильтра</param>
        /// <param name="b">Массив коэффициентов реверсивного фильтра</param>
        public void Initialize(double[] a, double[] b)
        {
            if(a[0] == 0)
                throw new ArgumentOutOfRangeException("a", "Параметр a[0] не доллжен быть равен 0");

            var lv_Order = Math.Max(a.Length, b.Length);
            if(lv_Order != Order)
                Initialize(lv_Order);
            else
                Initialize();

            f_a = new double[lv_Order];
            f_b = new double[lv_Order];
            Array.Copy(a, f_a, a.Length);
            Array.Copy(b, f_b, b.Length);


            if(f_a[0] == 1) return;
            var lv_a = a[0];
            f_a[0] /= lv_a;
            for(var i = 1; i < lv_Order; i++)
            {
                f_a[i] /= lv_a;
                f_b[i] /= lv_a;
            }
        }

        /// <summary>Обработка единичного отсчёта сигнала</summary>
        /// <param name="x_n">Единичный отсчёт входного воздействия</param>
        /// <returns>Отклик фильтра на входное воздействие</returns>
        public override double Process(double x_n)
        {
            return Process(x_n, f_a, f_b, f_DelayLine);
        }

        protected static double Process(double x_n, double[] a, double[] b, DelayLine Line)
        {
            Contract.Requires(a != null, "Отсутствует массив коэффициентов полинома числителя");
            Contract.Requires(a.Length > 0, "Степень полинома а должна быть больше 0");
            
            return a != null && b != null ? Process_BIH(x_n, a, b, Line)
                    : (b == null ? Process_Recurs(x_n, a, Line) : Process_KIH(x_n, b, Line));
        }

        /// <summary>Обработка входного воздействия в КИХ режиме</summary>
        /// <param name="x_n">Единичное входное воздействие</param>
        /// <param name="b">Массив коэффициентов прямого фильтра</param>
        /// <param name="Line">Линия задержки фильтра</param>
        /// <returns>Отклик фильтра</returns>
        private static double Process_KIH(double x_n, double[] b, DelayLine Line)
        {
            var lv_KihSumm = .0;
            Line.AddFirst(x_n);
            var i_Length = Line.Length;
            for(var i = 0; i < i_Length; i++)
                lv_KihSumm += Line[i] * b[i];
            return lv_KihSumm;
        }

        /// <summary>Обработка рекурсивным фильтром</summary>
        /// <param name="x_n">Единичный отсчёт входного воздействия</param>
        /// <param name="a">Коэффициенты рекурсивного фильтра</param>
        /// <param name="Line">Линия задержки фильтра</param>
        /// <returns></returns>
        private static double Process_Recurs(double x_n, double[] a, DelayLine Line)
        {
            var lv_RecursSumm = x_n;
            var i_Length = Line.Length;
            for(var i = 1; i < i_Length; i++)
                lv_RecursSumm -= Line[i - 1] * a[i];
            Line.AddFirst(lv_RecursSumm);
            return lv_RecursSumm;
        }

        /// <summary>Обработка входного воздействия фильтром с бесконечной импульсной эарактеристикой</summary>
        /// <param name="x_n">Единичный отсчёт входного водействия</param>
        /// <param name="a">Масив коэффициентов прямого фильтра</param>
        /// <param name="b">Массив коэффициентов рекурсивного фильтра</param>
        /// <param name="Line">Линия задержки фильтра</param>
        /// <returns>Отклик фильтра</returns>
        private static double Process_BIH(double x_n, double[] a, double[] b, DelayLine Line)
        {
            Contract.Requires(a != null && a.Length > 0);
            var lv_KihSumm = .0;
            var lv_RecursSumm = x_n;
            var i_Length = Line.Length;
            for(var i = 1; i < i_Length; i++)
            {
                var z = Line[i - 1];
                lv_KihSumm += z * b[i];
                lv_RecursSumm -= z * a[i];
            }
            Line.AddFirst(lv_RecursSumm);
            lv_KihSumm += lv_RecursSumm * b[0];
            return lv_KihSumm;
        }
    }
}